#include "fishbot_setparam/nav2param_manager.h"

void Nav2ParamManager::reloadParameters(const std::string &yaml_path)
{
    try
    {
        YAML::Node config = YAML::LoadFile(yaml_path);
        RCLCPP_INFO(node_->get_logger(), "Loading parameters from: %s", yaml_path.c_str());
        /**
         e.g.
         amcl:
             kld_error: 0.05
             max_particles: 3000
             transform_tolerance: 0.2

         node_entry.first 为 amcl
         node_entry.second 为kld_error: 0.05
                             max_particles: 3000
                             transform_tolerance: 0.2
         */
        for (const auto &node_entry : config)
        {
            const std::string node_name = node_entry.first.as<std::string>();
            processNodeParameters(node_name, node_entry.second);
        }
    }
    catch (const YAML::Exception &e)
    {
        RCLCPP_ERROR(node_->get_logger(), "YAML parsing error: %s", e.what());
    }
}

void Nav2ParamManager::processNodeParameters(const std::string &node_name, const YAML::Node &params_node)
{
    // 创建client
    auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
        node_, node_name);

    if (!parameters_client->service_is_ready())
    {
        if (!parameters_client->wait_for_service(std::chrono::seconds(2)))
        {
            RCLCPP_WARN(node_->get_logger(), "Service unavailable for node: %s", node_name.c_str());
            return;
        }
    }
    // parameters的形式为("scan_topic", "scan")
    std::vector<rclcpp::Parameter> parameters;
    try
    {
        for (const auto &param_entry : params_node)
        {
            /**
                e.g.
                   amcl:
                       kld_error: 0.05
                       max_particles: 3000
                       transform_tolerance: 0.2
                   param_name 为 kld_error
                   param_value 为 0.05
            */
            const std::string param_name = param_entry.first.as<std::string>();
            const YAML::Node param_value = param_entry.second;
            // 将YAML节点转换为字符串
            YAML::Emitter emitter;
            emitter << param_value;
            std::string value_str = emitter.c_str();

            // 使用c_str()转换字符串并添加换行控制
            RCLCPP_INFO(
                node_->get_logger(),
                "change parameter %s: %s",
                param_name.c_str(), // 转换std::string为C字符串
                value_str.c_str()   // 转换YAML内容为C字符串
            );
            // 如果param_value不是一个数组
            if (param_value.IsScalar())
            {
                parameters.push_back(parseScalarParameter(param_name, param_value));
            }
            // 如果param_value是一个数组
            else if (param_value.IsSequence())
            {
                parameters.push_back(parseArrayParameter(param_name, param_value));
            }
            else
            {
                RCLCPP_WARN(node_->get_logger(), "Unsupported parameter type for %s/%s",
                            node_name.c_str(), param_name.c_str());
            }
        }
    }
    catch (const YAML::Exception &e)
    {
        RCLCPP_ERROR(node_->get_logger(), "Parameter parsing error: %s", e.what());
        return;
    }

    auto future = parameters_client->set_parameters(parameters);
    // handleParameterFuture(future, node_name);
}
/**
 * @brief 处理double int bool string四种非数组格式 ---   name:value
 *
 * @param name
 * @param value
 * @return rclcpp::Parameter
 */
rclcpp::Parameter Nav2ParamManager::parseScalarParameter(const std::string &name, const YAML::Node &value)
{
    try
    {
        return {name, value.as<double>()};
    }
    catch (...)
    {
        try
        {
            return {name, value.as<int>()};
        }
        catch (...)
        {
            try
            {
                return {name, value.as<bool>()};
            }
            catch (...)
            {
                return {name, value.as<std::string>()};
            }
        }
    }
}
/**
 * @brief 处理vector类型 --- name:value ---  critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
 *
 * @param name
 * @param value
 * @return rclcpp::Parameter
 */
rclcpp::Parameter Nav2ParamManager::parseArrayParameter(const std::string &name, const YAML::Node &value)
{
    try
    {
        return {name, value.as<std::vector<double>>()};
    }
    catch (...)
    {
        try
        {
            return {name, value.as<std::vector<int>>()};
        }
        catch (...)
        {
            try
            {
                return {name, value.as<std::vector<bool>>()};
            }
            catch (...)
            {
                return {name, value.as<std::vector<std::string>>()};
            }
        }
    }
}
/**
 * @brief 回调
 *
 * @param future
 * @param node_name
 */
void Nav2ParamManager::handleParameterFuture(std::shared_future<std::vector<rcl_interfaces::msg::SetParametersResult>> future,
                                             const std::string &node_name)
{
    auto future_callback = [node_name, logger = node_->get_logger()](auto future_result)
    {
        const auto results = future_result.get();
        for (const auto &result : results)
        {
            if (!result.successful)
            {
                RCLCPP_ERROR_STREAM(logger, "Failed to set parameter for " << node_name
                                                                           << ": " << result.reason);
            }
        }
        RCLCPP_INFO(logger, "Parameters updated for node: %s", node_name.c_str());
    };

    // future.wait();
    future_callback(future);
}